#ifndef __CONTROLLER_BASE_H__
#define __CONTROLLER_BASE_H__

#include <ros/ros.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Path.h>

#include "map_base.h"
#include "core/common/core.h"
#include "core/convert/msg_convert.h"
#include "core/utils/graphis_utils.h"
#include "core/utils/math_utils.h"
// #include "perception/costmap2d/costmap_2d.h"
// #include "perception/costmap_2d/costmap_2d_ros.h"
#include "path_generate/path_base.h"
#include "path_generate/p2p/jps.h"
#include "path_generate/p2p/astar.h"

using namespace std;

// 控制器类型
enum ContrllerType{
    DWA = 1,
    Pure_Pursuit = 2,
    PID = 3,
    MPC = 4,
};

// 控制器默认全局配置参数
struct ContrllerBaseConfig{
    double max_vel_x = 0.5;
    double min_vel_x = 0.0;
    double max_vel_yaw = 1.0;
    double min_vel_yaw = -1.0;
    double linear_acc = 0.5;
    double angular_acc = 1.0;
    double goal_distance_tolerance = 0.1;
    double goal_angle_tolerance = utils::math::toRad(10);
    double lookahead_time = 1.5;
    double min_lookahead_distance = 0.5;
    double max_lookahead_distance = 1.5;
    double avoidance_distance = 1.0;    //避障距离
    int controller_frequency = 20;  //20Hz
};

class ControllerBase{
    public:
        ControllerBase();
        ~ControllerBase();

        // 计算速度
        virtual mc::status computeTwist(geometry_msgs::PoseStamped goal, geometry_msgs::Twist curr_vel,
            geometry_msgs::Twist& cmd_vel) = 0;

        /*************************************************
        * 位姿转换
        *************************************************/
        // 获取TF位姿
        bool getTFPose(tf2_ros::Buffer* tf_buffer, geometry_msgs::PoseStamped& global_pose, 
            string father_frame, string child_frame, ros::Time stamp);
        // 获取机器人当前位姿
        bool getRobotPose(tf2_ros::Buffer* tf_buffer, geometry_msgs::PoseStamped& global_pose);
        // 全局位姿转局部
        geometry_msgs::PoseStamped convertGlobalPoseToLocal(geometry_msgs::PoseStamped global_pose, 
            geometry_msgs::PoseStamped robot_pose);
        // 局部位姿转全局
        geometry_msgs::PoseStamped convertLocalPoseToGlobal(geometry_msgs::PoseStamped local_pose, 
            geometry_msgs::PoseStamped robot_pose);
        // 全局路径转局部
        vector<geometry_msgs::PoseStamped> convertGlobalPathToLocal(vector<geometry_msgs::PoseStamped> global_path, 
            geometry_msgs::PoseStamped robot_pose);
        // 局部路径转全局
        vector<geometry_msgs::PoseStamped> convertLocalPathToGlobal(vector<geometry_msgs::PoseStamped> local_path, 
            geometry_msgs::PoseStamped robot_pose);

        /*************************************************
        * 辅助函数
        *************************************************/
        // 旋转至目标角度
        void rotateToTGoal(geometry_msgs::PoseStamped robot_pose,
            geometry_msgs::PoseStamped goal_pose, geometry_msgs::Twist& cmd_vel);
        // 计算最大减速距离
        double maxSpeedToStop(double vel, double acc);
        // 计算直线刹车距离 v^2 - 0 = 2as
        double maxStopDistance(geometry_msgs::Twist cmd_vel, double acc);

        // 检测目标点是否到达  距离判断
        bool isPositionReached(geometry_msgs::PoseStamped robot_pose, 
            geometry_msgs::PoseStamped goal_pose);
        // 检测目标点是否到达  角度判断
        bool isAngleReached(geometry_msgs::PoseStamped robot_pose, 
            geometry_msgs::PoseStamped goal_pose);
        // 检测目标点是否到达
        bool isGoalReached(geometry_msgs::PoseStamped robot_pose, geometry_msgs::PoseStamped goal_pose);

        // 路径裁剪
        void pruningLocalPath(vector<geometry_msgs::PoseStamped>& global_path, 
            vector<geometry_msgs::PoseStamped>& local_path, double limit_x, double limit_y);
        // 发布局部路径
        void publishLocalPath(vector<geometry_msgs::PoseStamped> local_path);
        // 获取前瞻距离
        double getLookAheadDistance(double vt);
        // 获取前瞻目标点
        bool getLookaheadGoal(vector<geometry_msgs::PoseStamped> local_path, 
            double lookahead_distance, geometry_msgs::PoseStamped& lookahead_goal, int& index);
        // 发布前瞻目标点
        void publishLookaheadGoal(geometry_msgs::PoseStamped lookahead_goal);

        // 获取距离最近的点
        bool getClosedPose(vector<geometry_msgs::PoseStamped> path, int& closest_index);
        // 计算距离平方
        double squarePose(geometry_msgs::PoseStamped a, geometry_msgs::PoseStamped b);
        // 校验当前点到目标点之间是否存在障碍物
        bool isPathCollision(vector<geometry_msgs::PoseStamped> local_path);
        // 重新生成局部路径
        bool replanLocalPath(vector<geometry_msgs::PoseStamped>& global_path, vector<geometry_msgs::PoseStamped>& local_path, 
            geometry_msgs::PoseStamped robot_pose);

        // // 更新代价地图
        // void updateCostmapData();
        // bool valueIsValid(unsigned char mx, unsigned char my);

        void updateCostmapData(boost::shared_ptr<nav_msgs::OccupancyGrid> costmap_ptr);
        void updateRobotPose(geometry_msgs::PoseStamped robot_pose);

        /*************************************************
        * 速度控制
        *************************************************/
        // 急停控制
        void e_stop();  
        // 减速控制
        void slow_down(double controller_frequency);
        // 停止控制
        void sendEmptyTwist();
        // 发送速度
        void sendVelocity(geometry_msgs::Twist& cmd_vel);
        

        // 符号判断
        inline float sign(float x){
            return x < 0.0 ? -1.0 : 1.0;
        }


        // 获取机器人原点位姿点 0.0， 0.0， 0.0
        inline geometry_msgs::PoseStamped getOriginPose(){
            return localOriginPose_;
        }

    protected:
        ContrllerBaseConfig baseConfig_;
        string baseFrameId_;
        geometry_msgs::Twist currCmdVel_;   // 当前速度
        geometry_msgs::PoseStamped localOriginPose_;
        boost::shared_ptr<PlannerBase> path_planner_ptr_;
        geometry_msgs::PoseStamped robot_posestamp_;

        // boost::shared_ptr<costmap_2d::Costmap2DROS> costmapLocalPtr_;
        // nav_msgs::OccupancyGrid local_map_;
        // 更新
        MapBase map_base_;

    private:
        ros::NodeHandle nh_;
        ros::Publisher speedPub_;
        ros::Publisher pathLocalPub_;
        ros::Publisher poseLookaheadPub_;
        ros::Publisher poseReplanPub_;
};

#endif